#include <FastSerial.h>
#include <avr/eeprom.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include "ppm.h"


#if defined(ARDUINO) && ARDUINO >= 100
#include "Arduino.h"
#else
#include "wiring.h"
#endif

ppm_setting_t ppm_setting_eeprom EEMEM;

static const int16_t PPM_MAX = 550;
static const int16_t PPM_NEUTRAL = 1520;
static const int16_t PPM_CONSTANT = 400;
static const int16_t PPM_CORR_LARGE = 7; //PPM����ɱ���������
static const int16_t PPM_CORR_SMALL = 4; //PPM���������������
static const int16_t PPM_FRAME = 22000;

const uint8_t PPM_CHANNELS = 8;

const int16_t PPM::TRIM_MAX = 120;
const int16_t PPM::TRIM_MIN = -120;
ppm_setting_t PPM::setting;
uint16_t PPM::input[8] = {PPM_NEUTRAL, PPM_NEUTRAL, PPM_NEUTRAL, PPM_NEUTRAL, PPM_NEUTRAL, PPM_NEUTRAL, PPM_NEUTRAL, PPM_NEUTRAL};
uint16_t PPM::output[8] = {PPM_NEUTRAL, PPM_NEUTRAL, PPM_NEUTRAL, PPM_NEUTRAL, PPM_NEUTRAL, PPM_NEUTRAL, PPM_NEUTRAL, PPM_NEUTRAL};
uint16_t PPM::servo_out[3] = {PPM_NEUTRAL, PPM_NEUTRAL, PPM_NEUTRAL};
bool PPM::ppm_in = false;

#define PPM_H PORTD |= (1<<2)
#define PPM_L PORTD &= ~(1<<2)

extern uint32_t system_ms;
/*
void PPM_setting::reset()
{
    memset(this, 0, sizeof(PPM_setting));
    atv[0] = atv[1] = atv[2] = 100;
}*/

void PPM::init()
{
    DDRD |= (1 << 2);
    DDRD &= ~( 1 << 3);
    PORTD |= (1 << 3);

    eeprom_read_block(&setting, &ppm_setting_eeprom, sizeof(setting));

    for (uint8_t c = 0; c < 9; c++)
        PPM::input[c] = PPM_NEUTRAL;
    for (uint8_t c = 0; c < 3; c++)
        PPM::servo_out[c] = PPM_NEUTRAL;

    setting.init();
/*    if(get_radio_type() == RADIO_FUTABA)
        PPM_H;
    else
        PPM_L;    //JR
*/

    EICRA |= (1<<ISC11);
//    if (get_mixing())
//        EIMSK |= (1<<INT1);

    TIMSK1 = 0x01;
    TCCR1A = 0x00;
    TCCR1B = 0x02;
}

void PPM::load_setting()
{
    if (eeprom_read_byte(&ppm_setting_eeprom.mark) == 'P')
        eeprom_read_block(&setting, &ppm_setting_eeprom, sizeof(setting));

    if(get_radio_type() == RADIO_FUTABA)
        PPM_H;
    else
        PPM_L;    //JR

    if (get_mixing())
        EIMSK |= (1<<INT1);
}

void PPM::save_setting()
{
    eeprom_write_block(&setting, &ppm_setting_eeprom, sizeof(ppm_setting_t));
}

void PPM::write_servo(uint8_t servo, int16_t angle_10)
{
    if (get_channel(servo) > 0)
    {
        int16_t pwm = ((int32_t)(angle_10)) * get_speed(servo) / 100;
        if (get_reverse(servo))
            pwm = PPM_NEUTRAL - pwm - get_trim(servo);
        else
            pwm = PPM_NEUTRAL + pwm - get_trim(servo);
        //write_channel(get_channel(servo), pwm);
        servo_out[servo] = constrain(pwm, get_min(servo), get_max(servo));
    }
}

void PPM::transfer()
{
//    static uint32_t last = 0;
    uint8_t oldSREG = SREG;
    cli();
    for (uint8_t c = 0; c < PPM_CHANNELS; c++)
        output[c] = input[c];
    SREG = oldSREG;

    for (uint8_t c = 0; c < 3; c++)
    {
        if (get_channel(c) > 0)
            output[get_channel(c)-1] = servo_out[c];
    }
    /*
    */
}


static inline void set_timer1(uint16_t value)
{
    uint16_t v = 0xFFFF - (value << 1);
    TCNT1H = v >> 8;
    TCNT1L = v;
}

//PPM���,��ʱ��1�ж����
//SIGNAL(SIG_OVERFLOW1)


SIGNAL(TIMER1_OVF_vect)
{
    static bool ppm_sign = false;
    static uint8_t ppm_index = 0;
    static uint16_t ppm_all = 0;

    if(ppm_sign)//������
    {
        if(PPM::get_radio_type() == RADIO_FUTABA)
            PPM_L;  //Futaba
        else
            PPM_H;           //JR
        set_timer1(PPM_CONSTANT - PPM_CORR_SMALL);//PPM_Constant - PPM_Correct_Small);
        ppm_sign = false;
        //ppm_index ++;
    }
    else
    {
        if(PPM::get_radio_type() == RADIO_FUTABA)
            PPM_H; //Futaba
        else
            PPM_L;          //JR

        if(ppm_index < PPM_CHANNELS)
        {
            set_timer1(PPM::get_channel_pwm(ppm_index) - PPM_CONSTANT - PPM_CORR_LARGE);
            ppm_all += PPM::get_channel_pwm(ppm_index);
            ppm_index ++;
        }
        else
        {
            set_timer1(PPM_FRAME - ppm_all - PPM_CONSTANT - PPM_CORR_LARGE);
            ppm_all = 0;
            ppm_index = 0;
            PPM::transfer();
        }
        ppm_sign = true;
    }
}


SIGNAL(INT1_vect)
{
    static uint32_t last = 0;
    static uint8_t index = 0;

    uint32_t curr = micros();
    uint32_t width = (uint32_t)(curr - last) + 1;
    last = curr;

    if (width > 3000 && width < PPM_FRAME)
        index = 0;
    else if (width < 3000 && width > 600 && index < PPM_CHANNELS)
    {
        PPM::write_channel_pwm(index, width);
        index++;
        PPM::ppm_in = true;
    }
}

